

/**
 * Holds functions for manipulating force structures.
 */

#include <swarm.h>

#include <stdlib.h>
#include <string.h>

unsigned int _sw_next_force_id = 0;

unsigned int sw_fid(){

  return _sw_next_force_id++;

}

sw_force *sw_new_force(){

  sw_force *f = (sw_force *) malloc(sizeof(sw_force));
  memset(f, 0, sizeof(sw_force));
  f->force_type = sw_fid();
  return f;

}

void sw_print_force(sw_force *f){

  printf("[swarm] Force: name=%s order=%lf magnitude=%lf\n",
	 f->name, f->force_order, f->force_constant);
  
}

void sw_swarm_add_force(sw_swarm *sw, sw_force *force){

  /* make a new array to hold the previous forces and this new one. */
  sw_force *new_list = (sw_force *)malloc(sizeof(sw_force) * (sw->f_len + 1));
  
  if (sw->forces){
    memcpy(new_list, sw->forces, sizeof(sw_force) * sw->f_len);
  }
  new_list[sw->f_len++] = *force;
  if (sw->forces) free(sw->forces);
  sw->forces = new_list;

}

int sw_find_force_offset(sw_swarm *sw, char *name){

  int i = 0;
  for ( ; i < sw->f_len; i++){
    if (strncmp(sw->forces[i].name, name, strlen(name)) == 0){
      return i;
    }
  }

  return -1;

}

void sw_calc_insect_force(sw_swarm *sw, sw_insect *ins){

  /* This will update the net force in the specified insect based on */
  /* where the other insects are relative to this one.               */

  pthread_mutex_lock(&ins->lock);

  int i = 0;
  for ( ; i < sw->swarm_size; i++){
    
    // Calc force based on each other insect.
    sw_insect *i_tmp = sw->swarm[i];

    sw_xmm_vec f;
    sw_xmm_vec r = ins->loc;
    double dist;

    _sw_distance(&i_tmp->loc, &ins->loc, &dist);

    _sw_vec_sub(&r, &i_tmp->loc); 

    sw_xmm_vec r_hat = r;
    _sw_scale(r_hat, 1/dist);
    

  }

  pthread_mutex_unlock(&ins->lock);

}
